package org.sunspotworld;

import com.sun.spot.core.peripheral.Spot;
import com.sun.spot.core.peripheral.TimeoutException;
import com.sun.spot.core.resources.Resources;
import com.sun.spot.core.resources.transducers.IAccelerometer3D;
import com.sun.spot.core.resources.transducers.ISwitch;
import com.sun.spot.core.resources.transducers.ITriColorLED;
import com.sun.spot.core.resources.transducers.LEDColor;
import com.sun.spot.core.util.Utils;
import com.sun.spot.edemo.EDemoBoard;
import com.sun.spot.ieee_802_15_4_radio.ChannelBusyException;
import com.sun.spot.ieee_802_15_4_radio.IRadioPolicyManager;
import com.sun.spot.ieee_802_15_4_radio.util.IEEEAddress;
import com.sun.spot.multihop.io.j2me.radiogram.Radiogram;
import com.sun.spot.multihop.io.j2me.radiogram.RadiogramConnection;
import javax.microedition.io.Connector;
import javax.microedition.midlet.MIDlet;
import javax.microedition.midlet.MIDletStateChangeException;
import java.io.IOException;
import java.util.Random;
import javax.microedition.io.Datagram;
import javax.microedition.io.DatagramConnection;

/**
 * A simple remote that controls the hands on lab trackbot.
 *
 * @author Luis Fernando Ascanio
 * @author Parsifal D'Sola
 */
public class SPOTcontrol extends MIDlet {
    private static final int CHANNEL = 20;  // 16, 20

    public static final double ACCELEROMETER_DEAD_ZONE = 0.2;
    public static final double MAX_ACCELEROMETER_VALUE = 0.7;
    public static final double ACCELEROMETER_RANGE = MAX_ACCELEROMETER_VALUE - ACCELEROMETER_DEAD_ZONE;
    private ITriColorLED [] leds = EDemoBoard.getInstance().getLEDs();
    private IRadioPolicyManager rpm = (IRadioPolicyManager)Resources.lookup(IRadioPolicyManager.class);
    private IEEEAddress ourAddr = new IEEEAddress(rpm.getIEEEAddress());
    public static final short PAN_ID = IRadioPolicyManager.DEFAULT_PAN_ID;
    private static final long SERVER_CHECK_INTERVAL = 10000;    // = 10 seconds
    
    /** Client command to locate a display server. */
    private static final byte LOCATE_SERVER_REQ  = 1;    // sent to display host
    /** Host command to indicate it is restarting. */
    private static final byte SERVER_RESTART     = 2;
    /** Host command to indicate it is quiting. */
    private static final byte SERVER_QUITTING    = 3;
     /** Host reply to indicate it is available. */
    private static final byte SERVER_AVAIL_REPLY = 11;
    private boolean isOn = true;
    private boolean first = true;

    private boolean connected = false;
    private long serverAddress = 0;
    private long ourMacAddress;
    private Random random;
    private RadiogramConnection connection;
    private IAccelerometer3D accelerometer;
    private ISwitch[] switches = EDemoBoard.getInstance().getSwitches();
    
    protected void destroyApp(final boolean b) throws MIDletStateChangeException {
    }
    
    protected void pauseApp() {
    }
    
    protected void startApp() throws MIDletStateChangeException {
        rpm.setChannelNumber(CHANNEL);
        
        connection = null;
        
        
        leds[0].setColor(LEDColor.RED);                // set color to moderate red
        leds[1].setColor(LEDColor.BLUE);               // set color to moderate red
        leds[2].setColor(LEDColor.BLUE);               // set color to moderate red
        leds[3].setColor(LEDColor.BLUE);                // set color to moderate red
        leds[4].setColor(LEDColor.RED);               // set color to moderate red
        leds[5].setColor(LEDColor.RED);               // set color to moderate red
        leds[6].setColor(LEDColor.YELLOW);
        leds[7].setColor(LEDColor.YELLOW);
        
//        rpm = Spot.getInstance().getRadioPolicyManager();
        rpm.setPanId(PAN_ID);
        rpm.setOutputPower(31); 
            
        // Read the accelerometer values and send them to the trackbot
        accelerometer = EDemoBoard.getInstance().getAccelerometer();
        initialize();
        handleSwitches.start();
        clientLoop();
    }
    
    private Thread handleSwitches = new Thread() {
        public void run() {
            while (true) {
                isOn = isOn || switches[1].isClosed();
                isOn = isOn && switches[0].isOpen();
                Utils.sleep(100);
            }
        }
    };
    
    /**
     * Scales a value read from the accelerometer to a -100 to 100 range.
     *
     * @param accelerometerValue A value read from the accelerometer
     * @return The scaled value
     */
    public double scaleAccelerometerValue(final double accelerometerValue) {
        // Is it in the dead zone?
        if ((accelerometerValue <= ACCELEROMETER_DEAD_ZONE) && (accelerometerValue >= -ACCELEROMETER_DEAD_ZONE)) {
            return 0.0;
        }
        // Scale the value and return it, removing the dead zone
        if (accelerometerValue > 0.0) {
            return Math.min((accelerometerValue - ACCELEROMETER_DEAD_ZONE) / ACCELEROMETER_RANGE * 100, 100.0);
        } else {
            return Math.max((accelerometerValue + ACCELEROMETER_DEAD_ZONE) / ACCELEROMETER_RANGE * 100, -100.0);
        }
    }
    
    private void handleCommands() {
        // Calibrate the accelerometer
        try {
//            accelerometer.setRestOffsets();
            connection = (RadiogramConnection) Connector.open("radiogram://" + new IEEEAddress(serverAddress).asDottedHex() + ":24");
            leds[0].setOn();
        } catch (final IOException e) {
            e.printStackTrace();
        }
        
        int previousX = 0;
        int previousY = 0, k=1;
        
        // Loop reading values from the accelerometer
        while (true) {
            try {
                if (isOn) {
                    first = true;
                    leds[k].setOn();
                    int scaledX = (int) scaleAccelerometerValue(accelerometer.getAccelX());
                    int scaledY = (int) scaleAccelerometerValue(accelerometer.getAccelY());

                    // If the value changed, send it to the trackbot
                    if (scaledX != previousX || scaledY != previousY) {
                        // Create a radiogram for sending data to the trackbot
                        Radiogram radiogram = (Radiogram) connection.newDatagram(connection.getNominalLength());

                        // Fill the radiogram
                        radiogram.writeInt(scaledX);
                        radiogram.writeInt(scaledY);

                        // Send the radiogram to the trackbot
                        System.out.println("sending: X:" + scaledX + ", Y:" + scaledY);
                        connection.send(radiogram);
                        leds[k].setOff();

                        if(k==7)
                            k=0;
                        k++;

                        previousX = scaledX;
                        previousY = scaledY;
                    }
                } else {
                    if (first) {
                        // Create a radiogram for sending data to the trackbot
                        Radiogram radiogram = (Radiogram) connection.newDatagram(connection.getNominalLength());

                        // Fill the radiogram
                        radiogram.writeInt(0);
                        radiogram.writeInt(0);

                        // Send the radiogram to the trackbot
                        connection.send(radiogram);
                        leds[k].setOff();
                        first=false;
                    }
                }
                


                // Sleep a bit
                try {
                    Thread.sleep(100);
                } catch (final InterruptedException e) {
                }
            } catch (final IOException ex) {
                ex.printStackTrace();
            }
        }
    }
    
    public void initialize() {
        // currently just use default channel + pan ID
        // Spot.getInstance().getRadioPolicyManager().setChannelNumber(CHANNEL_NUMBER);
        rpm.setPanId(PAN_ID);

        ourMacAddress = new IEEEAddress(System.getProperty("IEEE_ADDRESS")).asLong();
        random = new Random(ourMacAddress);
    }
    
    private boolean locateDisplayServer (DatagramConnection txConn, Datagram xdg,
                                         RadiogramConnection rcvConn, Datagram rdg) {
        boolean result = false;
        serverAddress = 0;
        try {
            xdg.reset();
            xdg.writeByte(LOCATE_SERVER_REQ);        // packet type
            int retry = 0;
            while (retry < 5) {
                try {
                    txConn.send(xdg);               // broadcast remote print request
                    break;
                } catch (ChannelBusyException ex) {
                    retry++;
                    Utils.sleep(random.nextInt(10) + 2);  // wait a random amount before retrying
                }
            }
            try {
                while (true) {                      // loop until we either get a good reply or timeout
                    rdg.reset();
                    System.out.println("receiving...");
                    rcvConn.receive(rdg);           // wait until we receive a request
                    if (rdg.readByte() == SERVER_AVAIL_REPLY) { // type of packet
                    System.out.println("Received");
                        long replyAddress = rdg.readLong();
                        if (replyAddress == ourMacAddress) {
                            String addr = rdg.getAddress();
                            IEEEAddress ieeeAddr = new IEEEAddress(addr);
                            serverAddress = ieeeAddr.asLong();
                            result = true;
                            System.out.println("Whee!");
                        }
                    }
                }
            } catch (TimeoutException ex) { /* ignore - just return false */ }
        } catch (IOException ex)  { /* also ignore - just return false */ }

        return result;
    }
    
    private void clientLoop () {
        DatagramConnection txConn = null;
        RadiogramConnection rcvConn = null;
        connected = false;
        while (true) {   // this outer loop is for retrying if the server restarts
            try {
                leds[0].setRGB(50,0,0);     // Red = not active
                leds[0].setOn();
                txConn = (DatagramConnection)Connector.open("radiogram://broadcast:24");
                Datagram xdg = txConn.newDatagram(txConn.getMaximumLength()); 
                rcvConn = (RadiogramConnection)Connector.open("radiogram://:24");
                rcvConn.setTimeout(300);             // timeout in 300 msec
                Radiogram rdg = (Radiogram)rcvConn.newDatagram(rcvConn.getMaximumLength());

                while (true) {   // loop to locate a remote print server
                    int tries = 0;
                    boolean found = false;
                    leds[1].setRGB(60,40,0);     // Yellow = looking for display server
                    leds[1].setOn();
                    do {
                        found = locateDisplayServer(txConn, xdg, rcvConn, rdg);
                        Utils.sleep(20);         // wait 20 msecs
                        ++tries;
                    } while (!found && tries < 5);
                    leds[1].setOff();
                    if (found) {
                        connected = true;
                        break;
                    } else {
                        Utils.sleep(SERVER_CHECK_INTERVAL);  // wait a while before looking again
                    }
                }
            } catch (Exception ex) {
                System.out.println("Error trying to locate remote display server: " + ex.toString());
                ex.printStackTrace();
                connected = false;
            } finally {
                try {
                    leds[1].setOff();
                    txConn.close();
                    if (rcvConn != null) {
                        rcvConn.close();
                        rcvConn = null;
                    }
                } catch (IOException ex) { /* ignore */ }
            }

            if (connected) {
                handleCommands();
            }
        }
    }
    
}


